package de.h_da.fbi.cg.pointcloud.spatial;

import java.util.Arrays;
import java.util.Comparator;
import java.util.Map;
import java.util.PriorityQueue;

/**
 * @author Nicolas Gramlich
 * @since 19:43:01 - 11.11.2010
 * @param <C>
 * @param <P>
 * @param <V>
 */
public class NearestNeighborSearch<C extends Number & Comparable<C>, P extends IPoint<C>, V> {
	// ===========================================================
	// Constants
	// ===========================================================

	// ===========================================================
	// Fields
	// ===========================================================

	private boolean mExcludeQueryPoint;
	private int mNeighborCount;
	private double mMinDistance;
	private IDistanceAlgorithm<C, P> mDistanceAlgorithm;
	private PriorityQueue<Entry<C, P, V>> mPriorityQueue;
	private P mQuery;

	// ===========================================================
	// Constructors
	// ===========================================================

	public NearestNeighborSearch(final IDistanceAlgorithm<C, P> pDistanceAlgorithm) {
		this.mDistanceAlgorithm = pDistanceAlgorithm;
	}

	/**
	 * Constructs a NearestNeighbors instance using a {@link EuclideanDistance}
	 * instance to calculate distances between points.
	 */
	public NearestNeighborSearch() {
		this(new EuclideanDistance<C, P>());
	}

	// ===========================================================
	// Getter & Setter
	// ===========================================================

	// ===========================================================
	// Methods for/from SuperClass/Interfaces
	// ===========================================================

	// ===========================================================
	// Methods
	// ===========================================================

	private void find(final KDTree<C, P, V>.KDNode pKDNode) {
		if(pKDNode == null) {
			return;
		}

		final int discriminator = pKDNode.mDimension;
		final P point = pKDNode.getKey();
		double distanceSquared = this.mDistanceAlgorithm.distanceSquared(this.mQuery, point);

		if(distanceSquared < this.mMinDistance && (distanceSquared != 0.0 || !this.mExcludeQueryPoint)) {
			if(this.mPriorityQueue.size() == this.mNeighborCount) {
				this.mPriorityQueue.poll();
				this.mPriorityQueue.add(new NearestNeigborEntry(distanceSquared, pKDNode));
				this.mMinDistance = this.mPriorityQueue.peek().getDistanceSquared();
			} else {
				this.mPriorityQueue.add(new NearestNeigborEntry(distanceSquared, pKDNode));
				if(this.mPriorityQueue.size() == this.mNeighborCount) {
					this.mMinDistance = this.mPriorityQueue.peek().getDistanceSquared();
				}
			}
		}

		final double dp = this.mQuery.getCoordinate(discriminator).doubleValue() - point.getCoordinate(discriminator).doubleValue();

		distanceSquared = dp * dp;

		if(dp < 0) {
			this.find(pKDNode.mLower);
			if(distanceSquared < this.mMinDistance) {
				this.find(pKDNode.mHigher);
			}
		} else {
			this.find(pKDNode.mHigher);
			if(distanceSquared < this.mMinDistance) {
				this.find(pKDNode.mLower);
			}
		}
	}

	public void setDistanceAlgorithm(final IDistanceAlgorithm<C, P> pDistanceAlgorithm) {
		this.mDistanceAlgorithm = pDistanceAlgorithm;
	}

	/**
	 * Same as {@link #get get(tree, queryPoint, numNeighbors, true)}.
	 */
	public Entry<C, P, V>[] get(final KDTree<C, P, V> tree, final P queryPoint, final int pNeighborCount) {
		return this.get(tree, queryPoint, pNeighborCount, true);
	}

	/**
	 * @return An array containing the nearest neighbors and their distances
	 *         sorted by least distance to greatest distance. If no neighbors
	 *         are found, the array will have a length of zero.
	 */
	@SuppressWarnings("unchecked")
	public Entry<C, P, V>[] get(final KDTree<C, P, V> pTree, final P pQueryPoint, final int pNeighborCount, final boolean pExcludeQueryPoint) {
		this.mExcludeQueryPoint = pExcludeQueryPoint;
		this.mNeighborCount = pNeighborCount;
		this.mQuery = pQueryPoint;
		this.mMinDistance = Double.POSITIVE_INFINITY;

		this.mPriorityQueue = new PriorityQueue<Entry<C, P, V>>(pNeighborCount, new EntryComparator());

		if(pNeighborCount > 0) {
			this.find(pTree.mRoot);
		}

		final Entry<C, P, V>[] neighbors = new Entry[this.mPriorityQueue.size()];

		this.mPriorityQueue.toArray(neighbors);
		Arrays.sort(neighbors);

		this.mPriorityQueue = null;
		this.mQuery = null;

		return neighbors;
	}

	// ===========================================================
	// Inner and Anonymous Classes
	// ===========================================================

	public interface Entry<T extends Number & Comparable<T>, K extends IPoint<T>, V> {
		public double getDistance();
		public double getDistanceSquared();

		public Map.Entry<K, V> getMapEntry();
		public K getKey();
		public V getValue();
	}

	private final class NearestNeigborEntry implements Entry<C, P, V>, Comparable<Entry<C, P, V>> {
		double mDistanceSquared;
		Map.Entry<P, V> mNeighbor;

		NearestNeigborEntry(final double pDistanceSquared, final Map.Entry<P, V> pNeighbor) {
			this.mDistanceSquared = pDistanceSquared;
			this.mNeighbor = pNeighbor;
		}

		public double getDistance() {
			return StrictMath.sqrt(this.mDistanceSquared);
		}

		public double getDistanceSquared() {
			return this.mDistanceSquared;
		}

		public Map.Entry<P, V> getMapEntry() {
			return this.mNeighbor;
		}

		public int compareTo(final Entry<C, P, V> pOther) {
			final double d = pOther.getDistanceSquared();

			if(this.mDistanceSquared < d) {
				return -1;
			} else if(this.mDistanceSquared > d) {
				return 1;
			}

			return 0;
		}

		@Override
		public P getKey() {
			return this.getMapEntry().getKey();
		}

		@Override
		public V getValue() {
			return this.getMapEntry().getValue();
		}
	}

	private final class EntryComparator implements Comparator<Entry<C, P, V>> {
		public int compare(final Entry<C, P, V> pEntryA, final Entry<C, P, V> pEntryB) {
			final double distanceSquaredA = pEntryA.getDistanceSquared();
			final double distanceSquaredB = pEntryB.getDistanceSquared();

			if(distanceSquaredA < distanceSquaredB) {
				return 1;
			} else if(distanceSquaredA > distanceSquaredB) {
				return -1;
			} else {
				return 0;
			}
		}

		@Override
		public boolean equals(final Object pOther) {
			return (pOther != null && pOther == this);
		}
	}
}
